import cv2
import threading
import time
import queue
import os
import shutil
from datetime import datetime

class CameraRecorder:
    def __init__(self, camera_id, output_file, frame_queue):
        self.camera_id = camera_id
        self.output_file = output_file
        self.frame_queue = frame_queue
        self.running = False
        self.cap = None
        self.writer = None
        self.fps = 30
        
        self.frame_size = None
        
    def start(self):
        self.running = True
        self.cap = cv2.VideoCapture(self.camera_id)
        self.cap.set(cv2.CAP_PROP_AUTO_WB, 1)
        self.cap.set(cv2.CAP_PROP_WB_TEMPERATURE, 5000)
        if not self.cap.isOpened():
            raise IOError(f"Cannot open camera {self.camera_id}")
        actual_width = int(self.cap.get(cv2.CAP_PROP_FRAME_WIDTH))
        actual_height = int(self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
        self.frame_size = (actual_width, actual_height)
        actual_fps = self.cap.get(cv2.CAP_PROP_FPS)
        if actual_fps <= 0:
            actual_fps = self.fps
            
        print(f"Camera {self.camera_id}: Size={self.frame_size}, FPS={actual_fps}")

        fourcc_options = [
            cv2.VideoWriter_fourcc(*'mp4v'),
            cv2.VideoWriter_fourcc(*'X264'),
            cv2.VideoWriter_fourcc(*'MJPG'),
            cv2.VideoWriter_fourcc(*'XVID')
        ]
        
        os.makedirs(os.path.dirname(self.output_file), exist_ok=True)
        
        for fourcc_code in fourcc_options:
            try:
                self.writer = cv2.VideoWriter(
                    self.output_file, 
                    fourcc_code, 
                    actual_fps, 
                    self.frame_size
                )
                if self.writer.isOpened():
                    print(f"Using codec: {fourcc_code} for camera {self.camera_id}")
                    break
                else:
                    self.writer = None
            except:
                self.writer = None
        
        if not self.writer or not self.writer.isOpened():
            raise RuntimeError(f"Failed to create VideoWriter for camera {self.camera_id}")

        self.thread = threading.Thread(target=self._record)
        self.thread.start()
    
    def _record(self):
        frame_count = 0
        start_time = time.time()
        
        while self.running:
            ret, frame = self.cap.read()
            if not ret:
                print(f"Camera {self.camera_id} read error")
                break
                
            timestamp = time.time()
            self.frame_queue.put((timestamp, frame))
            
            try:
                self.writer.write(frame)
                frame_count += 1
            except Exception as e:
                print(f"Write error for camera {self.camera_id}: {e}")
                self._recreate_writer()
                continue
                
        elapsed = time.time() - start_time
        actual_fps = frame_count / elapsed if elapsed > 0 else 0
        print(f"Camera {self.camera_id}: Recorded {frame_count} frames at {actual_fps:.2f} FPS")
    
    def _recreate_writer(self):
        if self.writer:
            self.writer.release()
            self.writer = None
            
        try:
            self.writer = cv2.VideoWriter(
                self.output_file, 
                cv2.VideoWriter_fourcc(*'mp4v'), 
                self.fps, 
                self.frame_size
            )
            if not self.writer.isOpened():
                self.writer = None
        except:
            self.writer = None
            
        if not self.writer:
            print(f"Failed to recreate writer for camera {self.camera_id}")
    
    def stop(self):
        self.running = False
        if self.thread.is_alive():
            self.thread.join(timeout=2.0)
        if self.cap:
            self.cap.release()
        if self.writer:
            self.writer.release()

def synchronize_frames(queue1, queue2):
    max_queue_size = 100
    tolerance = 0.05 
    
    while True:
        if queue1.qsize() > max_queue_size:
            queue1.get()
        if queue2.qsize() > max_queue_size:
            queue2.get()
            
        if not queue1.empty() and not queue2.empty():
            ts1, frame1 = queue1.queue[0] 
            ts2, frame2 = queue2.queue[0]
            
            time_diff = ts1 - ts2
            
            if abs(time_diff) < tolerance:
                queue1.get()
                queue2.get()
                return frame1, frame2
            elif time_diff > 0:
                queue2.get()
            else:  
                queue1.get() 

def main():
    queue1 = queue.Queue(maxsize=1)
    queue2 = queue.Queue(maxsize=1)
    
    output_dir = "./recordings/video"
    os.makedirs(output_dir, exist_ok=True)
    
    timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
    output1 = os.path.join(output_dir, f"camera1_{timestamp}.mp4")
    output2 = os.path.join(output_dir, f"camera2_{timestamp}.mp4")
    
    recorder1 = CameraRecorder(0, output1, queue1)
    recorder2 = CameraRecorder(1, output2, queue2)
    
    try:
        recorder1.start()
        recorder2.start()
        print("Recording started. Press 'q' to stop...")
        
        cv2.namedWindow("Camera 1", cv2.WINDOW_NORMAL)
        cv2.namedWindow("Camera 2", cv2.WINDOW_NORMAL)
        
        while True:
            frame1, frame2 = synchronize_frames(queue1, queue2)
            frame1 = cv2.resize(frame1, (640, 480))
            frame2 = cv2.resize(frame2, (640, 480))
            cv2.imshow("Camera 1", frame1)
            cv2.imshow("Camera 2", frame2)

            if cv2.waitKey(1) & 0xFF == ord('q'):
                break
                
    except Exception as e:
        print(f"Error: {e}")
    finally:
        recorder1.stop()
        recorder2.stop()
        cv2.destroyAllWindows()
        print(f"Recording saved to:\n{output1}\n{output2}")

        total, used, free = shutil.disk_usage("/")
        print(f"Disk space: {free//(1024**3)}GB free")

if __name__ == "__main__":
    os.environ["OPENCV_FFMPEG_LOGLEVEL"] = "warning"
    
    print(f"OpenCV version: {cv2.__version__}")
    print(f"FFmpeg support: {cv2.videoio_registry.getBackendName(cv2.CAP_FFMPEG)}")
    
    main()
